import socket
import re
import time
from configparser import ConfigParser
from threading import Thread
# import os
socket.setdefaulttimeout(30)

config = ConfigParser()
config.read('./src/arm.ini', 'UTF-8')

PORT = 8080
FORMAT = 'utf-8'
SERVER = "192.168.58.2"
# 下位机的ip地址是192.168.58.2
ADDR = (SERVER, PORT)



origin_x = 0
origin_y = 0
origin_z = 0
origin_rx = 0
origin_ry = 0
origin_rz = 0


ask_count = 10
current_pen = 3

angle:str ='-180,15,0'

current = [0,0,0]
origin =    [float(config['communicate']['origin_x']),
            float(config['communicate']['origin_y']),
            float(config['communicate']['origin_z'])]

t_flag = 0

def link():
    global client
    client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    client.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    client.connect(ADDR)
    print("[CONNECTED]")


def send(msg):
    message = msg.encode(FORMAT)
    client.send(message)


def up():
    send("/f/bIII2III232III23IIIStartJOG(8,3,0,10,10,9)III/b/f")


def down():
    send("/f/bIII2III232III23IIIStartJOG(8,3,1,10,10,9)III/b/f")


def move(x,y,z,pen=1,speed=20): # absolute position
    global origin, current, angle
    current[0] = origin[0] + float(x)
    current[1] = origin[1] + float(y)
    current[2] = origin[2] + float(z)
    block1 = "/f/bIII1III351III"
    block3 = "III"
    if pen == 0:
        block4 = "MoveCart({" + str(current[0]) + "," + str(current[1]) + "," + str(current[2]) + ","+ "-180,15,0}" +f",0,0,{speed},10,100,-1.0,-1)"
    if pen == 1:
        block4 = "MoveCart({" + str(current[0]) + "," + str(current[1]) + "," + str(current[2]) + ","+ "-165,0,90}" +f",0,0,{speed},10,100,-1.0,-1)"
    if pen == 2:
        block4 = "MoveCart({" + str(current[0]) + "," + str(current[1]) + "," + str(current[2]) + ","+ "180,-15,180}" +f",0,0,{speed},10,100,-1.0,-1)"
    if pen == 3:
        block4 = "MoveCart({" + str(current[0]) + "," + str(current[1]) + "," + str(current[2]) + ","+ "165,0,-90}" +f",0,0,{speed},10,100,-1.0,-1)"
    block2 = str(len(block4))
    block5 = "III/b/f"
    quantum_msg = block1 + block2 + block3 + block4 + block5
    send(quantum_msg)
    print(f'[MOVE] {current[0]-origin[0]} {current[1]-origin[1]} {current[2]-origin[2]} {angle}')
    msg = ""
    msg = client.recv(2048)


def getinversekin(x,y,z):
    block1 = "/f/bIII1III377III"
    block3 = "III"
    block4 = f"GetInverseKin(0,{x+origin[0]},{y+origin[1]},{z+origin[2]},{angle},-1)"
    block2 = str(len(block4))
    block5 = "III/b/f"
    quantum_msg = block1 + block2 + block3 + block4 + block5
    msg = ""
    while "377" not in msg:
        send(quantum_msg)
        msg = client.recv(2048).decode(FORMAT)
    pattern = "III"
    result = [_.start() for _ in re.finditer(pattern, msg)]
    extract = msg[result[-2]+3:result[-1]]
    num = extract.split(',')
    return extract

def spline(pointslist):
    global angle
    block1 = "/f/bIII2III553III"
    block3 = "III"
    block4 = f"NewSplineStart()"
    block2 = str(len(block4))
    block5 = "III/b/f"
    quantum_msg = block1 + block2 + block3 + block4 + block5
    send(quantum_msg)
    msg = ''
    msg = client.recv(2048).decode(FORMAT)
    # print(msg)
    
    for point in pointslist:
        block1 = "/f/bIII2III555III"
        block3 = "III"
        block4 = f"NewSplinePoint({getinversekin(point[0],point[1],point[2])},{point[0]+origin[0]},{point[1]+origin[1]},{point[2]+origin[2]},{angle},0,0,100,100)"
        block2 = str(len(block4))
        block5 = "III/b/f"
        quantum_msg = block1 + block2 + block3 + block4 + block5
        send(quantum_msg)
        # time.sleep(1)
        msg = ''
        msg = client.recv(2048).decode(FORMAT)
        # print(msg)

    block1 = "/f/bIII2III554III"
    block3 = "III"
    block4 = f"NewSplineEnd(1,100)"
    block2 = str(len(block4))
    block5 = "III/b/f"
    quantum_msg = block1 + block2 + block3 + block4 + block5
    send(quantum_msg)
    msg = ''
    msg = client.recv(2048).decode(FORMAT)
    # print(msg)
    print(f'[SPLINE] {pointslist}')



def close():
    global client
    client.shutdown(socket.SHUT_RDWR)
    client.close()


def speed_is_zero():
    global ask_count
    msg = ''
    while f'{ask_count}' not in msg:
        time.sleep(1)
        send(f"/f/bIII{ask_count}III377III28IIIGetActualJointSpeedsDegree()III/b/f")
        msg = client.recv(2048).decode(FORMAT)
    ask_count = (ask_count+1)%80+10
    pattern = "III"
    result = [_.start() for _ in re.finditer(pattern, msg)]
    extract = msg[result[-2]+3:result[-1]]
    num = extract.split(',')
    speed_sum = 0
    for i in range(len(num)):
        speed_sum += float(num[i])
    return speed_sum < len(num) * 0.1

current_x=0
current_y=0
current_z=0

def func():
    global client
    global current_x
    global current_y
    global current_z
    global t_flag
    global current_pen
    count = 0
    with open('./src/command.drawin', "r") as file_obj:
        contents = file_obj.readlines()
        for command in contents:
            count +=1
            print(f"[COUNT:{count}],{command}")
            while t_flag==1:
                time.sleep(1)
            if t_flag==2:
                file_obj.close()
                t_flag = 0
                print('[FILE CLOSED]')
                break
            if "u" in command:
                current_z = 10
                move(current_x, current_y, current_z, current_pen, 80)
            if "d" in command:
                if current_x>0 and current_y >0:
                    current_pen = 0
                if current_x<0 and current_y >0:
                    current_pen = 1
                if current_x<0 and current_y<0:
                    current_pen =2
                if current_x>0 and current_y<0:
                    current_pen=3
                move(current_x, current_y, current_z, current_pen, 80)
                current_z = 0
                move(current_x, current_y, current_z, current_pen, 80)
            if "," in command:
                s = command.split(',')
                x = float(s[0])/4.0
                y = float(s[1].rstrip("\n"))/4.0
                current_x = x
                current_y = y
                move(current_x, current_y, current_z, current_pen)
            if 'end' in command:
                time.sleep(1)
                current_z = 40
                move(current_x, current_y, current_z, current_pen)
                while(not speed_is_zero()):
                    time.sleep(1)
                print("\n\n[FINISH] painting task finished\n\n")
        file_obj.close()
        t_flag = 0
        print('[FILE CLOSED]')

t = Thread(target=func)

def go():
    global t, t_flag
    if not t.is_alive():
        t = Thread(target=func)
        t.start()
        t_flag = 0
    else :
        t_flag = 1 - t_flag


def stop():
    global t_flag
    t_flag = 2

if __name__=='__main__':
    link()

    move(0,0,10,0,80)
    move(0,0,0,0,80)
    move(0,0,10,0,80)

    move(0,0,10,1,80)
    move(0,0,0,1,80)
    move(0,0,10,1,80)

    move(0,0,10,2,80)
    move(0,0,0,2,80)
    move(0,0,10,2,80)

    move(0,0,10,3,80)
    move(0,0,0,3,80)
    move(0,0,10,3,80)
    # go()
    # close()
